#include "mpx_supt.h"       // includes stdio, ctype, and string
#include "pqueue.h"

/************************ Utility Function Prototypes *************************/
int create_node(void *, ppqn, ppqn *);
int delete_node(ppq, ppqn);
int delete_nodes_recursively(ppq, ppqn);

int count_bits(unsigned long);
int find_match(ppq, ppqn, int, void **, void **);
int locate_by_index(int, int, ppqn *, ppqn *, int, int, ppqn **, ppqn **);
int reheap_down(ppq, ppqn);
int reheap_up(ppq, ppqn);

/***************************** Utility Functions ******************************/
int count_bits(unsigned long number) {
	int bits = 0;
	
	while (number > 0) {
		bits++;
		number >>= 1;
	}
	
	return bits;
}

int create_node(void *data, ppqn parent, ppqn *out) {
	int error_code = (out != NULL ? OK : ERR_PQUEUE_NULLARG);
	
	if (error_code == OK) {
		*out = (ppqn) sys_alloc_mem(sizeof(pqn));
		
		if (*out == NULL) {
			error_code = ERR_GEN_OUTOFMEMORY;
		} else {
			(*out)->left   = NULL;
			(*out)->parent = parent;
			(*out)->right  = NULL;
			(*out)->data   = data;
		}
	}
	
	return error_code;
}

int delete_node(ppq queue, ppqn to_delete) {
	int error_code = (
		(queue == NULL || to_delete == NULL) ?
		ERR_PQUEUE_NULLARG :
		OK
	);
	
	if (error_code == OK) {
		if (to_delete->data != NULL) {
			error_code = queue->delete_data(to_delete->data);
		}
		
		if (error_code == OK) {
			error_code = sys_free_mem(to_delete);
		}
	}
	
	return error_code;
}

int delete_nodes_recursively(ppq queue, ppqn to_delete) {
	int error_code = (
		(queue == NULL || to_delete == NULL) ?
		ERR_PQUEUE_NULLARG :
		OK
	);
	
	if (error_code == OK) {
		if (to_delete->right != NULL) {
			error_code = delete_nodes_recursively(queue, to_delete->right);
		}
		
		if (error_code == OK && to_delete->left != NULL) {
			error_code = delete_nodes_recursively(queue, to_delete->left);
		}
		
		if (error_code == OK) {
			error_code = delete_node(queue, to_delete);
		}
	}
	
	return error_code;
}

int find_data(ppqn node, void *data_to_match, ppqn *out) {
	int error_code = (
		out == NULL ?
		ERR_PQUEUE_NULLARG :
		ERR_PQUEUE_ITEMNOTFOUND
	);
	
	if (error_code == ERR_PQUEUE_ITEMNOTFOUND && node != NULL) {
		if (node->data == data_to_match) {
			*out = node;
			error_code = OK;
		} else if (
			(error_code = find_data(node->left, data_to_match, out)) != OK
		) {
			error_code = find_data(node->right, data_to_match, out);
		}
	}
	
	return error_code;
}

int find_match(ppq queue, ppqn node, int argc, void **argv, void **out) {
	int error_code = (
		queue == NULL || out == NULL ?
		ERR_PQUEUE_NULLARG :
		ERR_PQUEUE_ITEMNOTFOUND
	);
	
	if (error_code == ERR_PQUEUE_ITEMNOTFOUND && node != NULL) {
		if (queue->find(node->data, argc, argv)) {
			*out = node->data;
			error_code = OK;
		} else if (
			(error_code = find_match(
				queue,
				node->left,
				argc,
				argv,
				out
			)) != OK
		) {
			error_code = find_match(queue, node->right, argc, argv, out);
		}
	}
	
	return error_code;
}

int locate_by_index(
	int  target,
	int  depth_of_target,
	ppqn *address_of_current_node_pointer,
	ppqn *address_of_parent_node_pointer,
	int  current_index,
	int  current_depth,
	ppqn **out_target_address,
	ppqn **out_parent_address
) {
	int error_code = (
		address_of_current_node_pointer == NULL ?
		ERR_PQUEUE_NULLARG :
		OK
	);
	
	if (error_code == OK) {
		if (target == current_index) {
			*out_target_address = address_of_current_node_pointer;
			*out_parent_address = address_of_parent_node_pointer;
		} else if (*address_of_current_node_pointer == NULL) {
			error_code = ERR_PQUEUE_NULLARG;
		} else if (
			target >> (depth_of_target - current_depth - 1) == current_index * 2
		) {
			error_code = locate_by_index(
				target,
				depth_of_target,
				&((*address_of_current_node_pointer)->left),
				address_of_current_node_pointer,
				current_index * 2,
				current_depth + 1,
				out_target_address,
				out_parent_address
			);
		} else {
			error_code = locate_by_index(
				target,
				depth_of_target,
				&((*address_of_current_node_pointer)->right),
				address_of_current_node_pointer,
				current_index * 2 + 1,
				current_depth + 1,
				out_target_address,
				out_parent_address
			);
		}
	}
	
	return error_code;
}

int reheap_down(ppq queue, ppqn node) {
	int error_code = (queue == NULL ? ERR_PQUEUE_NULLARG : OK);
	
	if (
		error_code == OK &&
		node != NULL &&
		(node->left != NULL || node->right != NULL)
	) {
		ppqn larger;

		if (node->left == NULL) {
			error_code = ERR_PQUEUE_MALFORMEDTREE;
		} else if (
			node->right == NULL ||
			!queue->compare_nodes(node->right->data, node->left->data)
		) {
			larger = node->left;
		} else {
			larger = node->right;
		}
		
		if (
			error_code == OK &&
			!queue->compare_nodes(node->data, larger->data)
		) {
			void *swap   = node->data;
			node->data   = larger->data;
			larger->data = swap;
			
			error_code = reheap_down(queue, larger);
		}
	}
	
	return error_code;
}

int reheap_up(ppq queue, ppqn node) {
	int error_code = (queue == NULL || node == NULL ? ERR_PQUEUE_NULLARG : OK);
	
	if (error_code == OK) {
		if (
			node->parent != NULL &&
			queue->compare_nodes(node->data, node->parent->data)
		) {
			void *swap         = node->data;
			node->data         = node->parent->data;
			node->parent->data = swap;
			
			error_code = reheap_up(queue, node->parent);
		}
	}
	
	return error_code;
}

/**************** Priority Queue Construction and Destruction *****************/
int pq_init(
	ppq queue,
	int (*compare_nodes)(void *, void *),
	int (*delete_data)(void *),
	int (*find)(void *, int, void **)
) {
	int error_code = (
		(queue == NULL ||
		 compare_nodes == NULL ||
		 delete_data == NULL ||
		 find == NULL) ?
		ERR_PQUEUE_NULLARG :
		OK
	);
	
	if (error_code == OK) {
		queue->compare_nodes      = compare_nodes;
		queue->delete_data        = delete_data;
		queue->find               = find;
		queue->queue_hash_changed = FALSE;
		queue->candidates_length  = 0;
		queue->length             = 0;
		queue->root               = NULL;
		queue->next_candidates    = NULL;
	}
	
	return error_code;
}

int pq_free(ppq queue) {
	int error_code = pq_make_empty(queue);
	
	if (error_code == OK) {
		queue->compare_nodes = NULL;
		queue->delete_data   = NULL;
		queue->find          = NULL;
	}
	
	return error_code;
}

/********************* Priority Queue Observer Functions **********************/
int pq_is_empty(ppq queue) {
	return (queue->length == 0 ? TRUE : FALSE);
}

int pq_is_full(ppq queue) {
	int error_code;
	ppqn temp;
	
	error_code = create_node(NULL, NULL, &temp);
	if (error_code == OK) {
		error_code = delete_node(queue, temp);
	}
	
	if (error_code == OK) {
		error_code = FALSE;
	} else if (error_code == ERR_GEN_OUTOFMEMORY) {
		error_code = TRUE;
	}
	
	return error_code;
}

/******************** Priority Queue Transformer Functions ********************/
int pq_dequeue(ppq queue, void **out) {
	int error_code = (
		queue == NULL || out == NULL ?
		ERR_PQUEUE_NULLARG :
		(pq_is_empty(queue) == TRUE ? ERR_PQUEUE_EMPTYQUEUE : OK)
	);
	
	if (error_code == OK) {
		ppqn *delete_from;
		ppqn *parent;
		
		error_code = locate_by_index(
			queue->length,
			count_bits(queue->length),
			&(queue->root),
			NULL,
			1,
			1,
			&delete_from,
			&parent
		);
		
		if (error_code == OK) {
			*out = queue->root->data;
			queue->root->data = (*delete_from)->data;
			(*delete_from)->data = NULL;
			
			error_code = delete_node(queue, *delete_from);
			
			if (error_code == OK) {
				*delete_from = NULL;
				queue->length--;
				error_code = reheap_down(queue, queue->root);
				queue->queue_has_changed = TRUE;
			}
		}
	}
	
	return error_code;
}

int pq_enqueue(ppq queue, void *data) {
	int error_code = (queue == NULL ? ERR_PQUEUE_NULLARG : OK);
	
	if (error_code == OK) {
		ppqn *insert_at;
		ppqn *parent;
		
		error_code = locate_by_index(
			queue->length + 1,
			count_bits(queue->length + 1l),
			&(queue->root),
			NULL,
			1,
			1,
			&insert_at,
			&parent
		);
		
		if (error_code == OK) {
			error_code = create_node(data, *parent, insert_at);
			
			if (error_code == OK) {
				queue->length++;
				error_code = reheap_up(queue, *insert_at);
				queue->queue_has_changed = TRUE;
			}
		}
	}
	
	return error_code;
}

int pq_find(ppq queue, int argc, void **argv, void **out) {
	int error_code = (
		queue == NULL || out == NULL ?
		ERR_PQUEUE_NULLARG :
		(pq_is_empty(queue) == TRUE ? ERR_PQUEUE_ITEMNOTFOUND : OK)
	);
	
	if (error_code == OK) {
		error_code = find_match(queue, queue->root, argc, argv, out);
	}
	
	return error_code;
}

int pq_make_empty(ppq queue) {
	int error_code = (queue == NULL ? ERR_PQUEUE_NULLARG : OK);
	
	if (error_code == OK) {
		error_code = (
			queue->root == NULL ?
			OK :
			delete_nodes_recursively(queue, queue->root)
		);
		
		if (error_code == OK) {
			queue->length = 0;
			queue->root   = NULL;
			queue->queue_has_changed = TRUE;
		}
	}
	
	return error_code;
}

int pq_remove(ppq queue, void *data_to_remove) {
	int error_code = (
		queue == NULL ?
		ERR_PQUEUE_NULLARG :
		(pq_is_empty(queue) == TRUE ? ERR_PQUEUE_ITEMNOTFOUND : OK)
	);
	
	if (error_code == OK) {
		ppqn node_with_data;
		error_code = find_data(queue->root, data_to_remove, &node_with_data);
		
		if (error_code == OK) {
			ppqn *delete_from;
			ppqn *parent;
			
			error_code = locate_by_index(
				queue->length,
				count_bits(queue->length),
				&(queue->root),
				NULL,
				1,
				1,
				&delete_from,
				&parent
			);
			
			if (error_code == OK) {
				node_with_data->data = (*delete_from)->data;
				(*delete_from)->data = NULL;
				
				error_code = delete_node(queue, *delete_from);
				
				if (error_code == OK) {
					*delete_from = NULL;
					queue->length--;
					queue->queue_has_changed = TRUE;
					error_code = reheap_down(queue, node_with_data);
					
					if (error_code == OK) {
						error_code = reheap_up(queue, node_with_data);
					}
				}
			}
		}
	}
	
	return error_code;
}

/********************* Priority Queue Iteration Functions *********************/
int pq_start_iteration(ppq queue) {
	int error_code = (
		queue == NULL ?
		ERR_PQUEUE_NULL_ARG : (
			pq_is_empty(queue) == FALSE ? OK : ERR_PQUEUE_EMPTYQUEUE
		)
	);
	
	if (error_code == OK) {
		if (queue->next_candidates != NULL) {
			error_code = sys_free_mem(queue->next_candidates);
		}
		
		if (error_code == OK) {
			queue->next_candidates = (ppqn *) sys_alloc_mem(
				sizeof(ppqn) * queue->length
			);
			
			if (queue->next_candidates == NULL) {
				error_code = ERR_GEN_OUTOFMEMORY;
			} else {
				queue->queue_has_changed  = FALSE;
				queue->next_candidates[0] = queue->root;
				queue->candidates_length  = 1;
			}
		}
	}
	
	return error_code;
}

int pq_get_next(ppq queue, void **data) {
	int error_code = (
		(queue == NULL || data == NULL) ?
		ERR_PQUEUE_NULLARG : (
			queue->queue_has_changed == TRUE ?
			ERR_PQUEUE_QUEUECHANGED : (
				queue->candidates_length > 0 && queue->next_candidates != NULL ?
				OK :
				ERR_PQUEUE_ITERATIONNOTREADY
			)
		)
	);
	
	if (error_code == OK) {
		long i;
		long index = 0;
		ppqn node = queue->next_candidates[0];
		
		data = node->data;
		
		for (i = 1; i < queue->candidates_length; i++) {
			void *current = queue->next_candidates[i]->data
			if (queue->compare_nodes(current, data) == TRUE) {
				index = i;
				node  = queue->next_candidates[i];
				data  = current;
			}
		}
		
		queue->next_candidates[index] = queue->next_candidates[
			queue->candidates_length--
		];
		
		if (node->left != NULL) {
			queue->next_candidates[queue->candidates_length++] = node->left;
			
			if (node->right != NULL) {
				queue->next_candidates[queue->candidates_length++] =
					node->right;
			}
		} else if (node->right != NULL) {
			error_code = ERR_PQUEUE_MALFORMEDTREE;
		}
		
		if (
			error_code == OK &&
			queue->candidates_length == 0 &&
			(error_code = sys_free_mem(queue->next_candidates)) == OK
		) {
			queue->next_candidates = NULL;
		}
	}
	
	return error_code;
}

/************************************ main ************************************/
/*
#define REPORT(function, x) printf("%s: %d\n", #function, x)

int compare_nodes(void *data1, void *data2) {
	return data1 > data2;
}

int delete_data(void *data) {
	data = data;
	return OK;
}

int find(void *data, int argc, void **args) {
	argc = argc;
	// printf("%ld == %ld ? %u\n", data, *args, (data == *args));
	return (data == *args);
}

void recurse_over_tree(ppqn node) {
	if (node != NULL) {
		printf("(%d ", node->data);
		
		if (node->left != NULL) {
			recurse_over_tree(node->left);
			printf(" ");
		} else {
			printf("nil ");
		}
		
		if (node->right != NULL) {
			recurse_over_tree(node->right);
		} else {
			printf("nil");
		}
		
		printf(")");
	}
}

int main() {
	int  error_code, i;
	long *out;
	pq   queue;
	void *args;
	
	sys_init(MODULE_R1);
	
	error_code = pq_init(&queue, compare_nodes, delete_data, find);
	REPORT(pq_init, error_code);
	
	printf("\nTesting pq_enqueue...\n");
	for (i = 1; i < 32; i++) {
		error_code = pq_enqueue(&queue, (void *) i);
		
		if (error_code != OK) {
			printf(
				"pq_enqueue failed trying to enqueue %d: %d\n",
				i,
				error_code
			);
			sys_exit();
		}
	}
	
	printf("length: %d\n", queue.length);
	recurse_over_tree(queue.root);
	printf("\ndone!\n\n");
	
	printf("Testing pq_dequeue...\n");
	for (i = 0; i < 4; i++) {
		out = NULL;
		error_code = pq_dequeue(&queue, (void *) &out);
		
		if (error_code != OK) {
			printf(
				"pq_dequeue failed trying to dequeue %d: %d\n",
				i,
				error_code
			);
			sys_exit();
		}
		
		printf("dequeued %d\n", out);
	}
	
	printf("length: %ld\n", queue.length);
	recurse_over_tree(queue.root);
	printf("\ndone!\n\n");
	
	printf("Testing pq_find...\n");
	
	args = (void *) 1;
	error_code = pq_find(&queue, 1, &args, (void *) &out);
	
	if (error_code != OK && error_code != ERR_PQUEUE_ITEMNOTFOUND) {
		printf("pq_find failed trying to find %d: %d\n", args, error_code);
		sys_exit();
	} else {
		printf(
			"pq_find %ld: %d\n",
			args,
			(out == args)
		);
	}
	
	args = (void *) 24;
	error_code = pq_find(&queue, 1, &args, (void *) &out);
	
	if (error_code != OK && error_code != ERR_PQUEUE_ITEMNOTFOUND) {
		printf("pq_find failed trying to find %d: %d\n", args, error_code);
		sys_exit();
	} else {
		printf(
			"pq_find %ld: %d\n",
			args,
			(out == args)
		);
	}
	
	args = (void *) 31;
	error_code = pq_find(&queue, 1, &args, (void *) &out);
	
	if (error_code != OK && error_code != ERR_PQUEUE_ITEMNOTFOUND) {
		printf("pq_find failed trying to find %d: %d\n", args, error_code);
		sys_exit();
	} else {
		printf(
			"pq_find %ld: %d\n",
			args,
			(out == args)
		);
	}
	printf("done!\n\n");
	
	printf("Testing pq_remove...\n");
	
	printf("removing 4...\n");
	args = (void *) 4;
	error_code = pq_remove(&queue, args);
	if (error_code != OK){
		printf(
			"pq_remove failed attempting to remove %d: %d",
			args,
			error_code
		);
		sys_exit();
	}
	printf("length: %ld\n", queue.length);
	recurse_over_tree(queue.root);
	printf("\n\n");
	
	printf("removing 21...\n");
	args = (void *) 21;
	error_code = pq_remove(&queue, args);
	if (error_code != OK){
		printf(
			"pq_remove failed attempting to remove %d: %d",
			args,
			error_code
		);
		sys_exit();
	}
	printf("length: %ld\n", queue.length);
	recurse_over_tree(queue.root);
	printf("\n");
	
	printf("done!\n\n");
	
	error_code = pq_free(&queue);
	REPORT(pq_free, error_code);
	
	return 0;
}
//*/